/*
 *  lib-smv: Matrix - Vector Library targeted to Structural Mechanics Problems
 *  Copyright (C) 2006-2008  St.Brcic, Lj.Zugic
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
/** @file Jacobi.h
 *  @brief Definition of class Jacobi - solution of the eigenvalue problem
 *
 *  EIGENVALUE PROBLEMS:  Class JACOBI  
 *  STANDARD EIGENVALUE PROBLEMS A*x = L*x
 * 
 *  Jacobi transformation 
 */ 

#include "Jacobi.h"

namespace smv_lib
{

//! empty (default) constructor
Jacobi::Jacobi(void)
{
  isMatrix_A   = false;
  isMatrix_B   = false;
  isCalculated = false;
}

//! constructor for the standard problem
Jacobi::Jacobi(Matrix& a) : A(a)
{
  N = A.getRows();
  assert(N == A.getCols());
  isMatrix_A   = true;
  isMatrix_B   = false;
  isCalculated = false;
}

//! constructor for the generalized problem
Jacobi::Jacobi(Matrix& a, Matrix& b) : A(a), B(b)
{
  N = A.getRows();
  assert(N == A.getCols());
  int n = B.getRows();
  assert(n == B.getCols() && N == n);
  isMatrix_A   = true;
  isMatrix_B   = true;
  isCalculated = false;
}

//! destructor
Jacobi::~Jacobi(void)
{}

//! set Matrix A (for the standard problem)
void Jacobi::setMatrix_A(Matrix& a)
{
  A = a;
  N = a.getRows();
  assert(N == a.getCols());
  isMatrix_A   = true;
  isMatrix_B   = true;
  isCalculated = false;
}

//! set Matrices A and B (for the generalized problem)
void Jacobi::setMatrix_B(Matrix& a, Matrix& b)
{
  A = a;
  B = b;
  N = A.getRows();
  assert(N == A.getCols());
  int m = B.getRows();
  assert(m == B.getCols() && N == m);
  isMatrix_A   = true;
  isMatrix_B   = true;
  isCalculated = false;
}

//! check and calculate
void Jacobi::calculate()
{
  Matrix Modal(N,N,"J_Modal.txt");
  Vector Spektar(N,"J_Spec.txt");
  V = Modal;
  d = Spektar;
  //--- standard or generalized problem
  if (isMatrix_A == true && isMatrix_B == false)
    stJacobi();
  else if (isMatrix_A == true && isMatrix_B == true)
    genJacobi();
  //--- sort eigenvectors and eigenvalues
  eigsrt_Jacobi();
  isCalculated = true;
 
  //--- write obtained results to files
  V.Write();
  d.Write();
  cout << "\nEigen-Vectros are in the file J_MODAL.txt\n";
  cout << "\nEigen-Values are in the file J_SPEC.txt\n";
}

//! give the modal matrix
Matrix Jacobi::giveModalMatrix()
{
  Matrix Zero(N,N);
  if(isCalculated == true)
    return V;
  else
    return Zero;
}

//! give eigenvalues
Vector Jacobi::giveSpectrum()
{
  Vector zero(N);
  if(isCalculated == true)
    return d;
  else
    return zero;
}

//=========== STANDARD EIGENVALUE PROBLEMS  ----------------- private functions
//            (complete solution)
//----------- Standard Jacobi transformation
//
#define ROTATE(A,i,j,k,l)  g=A.getElement(i,j); h=A.getElement(k,l); \
                                 A.setElement(i,j,g-s*(h+g*tau)); \
                                         A.setElement(k,l,h+s*(g-h*tau));
//--------------------------------------------------------------------------
/**
**  \brief Standard Jacobi transformation
**
**   All eigenvalues and eigenvectors or a real symmetrix matrix
**   INPUT:
**   Matrix A(n,n) - real symmetrix matrix of order n
**   OUTPUT:
**   Matrix V(n,n) - eigenmatrix - modal matrix (columns of V are eigenvectors of A)
**   Vector d(n)   - eigenvalues - vector containing eigenvalues of Matrix A
**   Notes: 
**   - elements of input Matrix A above the diagonal are destroyed
**   - int "nrot" contains the number of Jacobi rotations
**/
//--------------------------------------------------------------------------
void Jacobi::stJacobi()
{
  int nrot = 0;
  int j, iq, ip, i;
  int n = A.getRows();
  cout<< n<<endl;
  cout<<" matrica A = "<<A<<endl;
  system("PAUSE");
  double tresh, theta, tau, t, sm, s, h, g, c;
  Vector* b = new Vector(n);
  assert(b != NULL);
  Vector* z = new Vector(n);
  assert(z != NULL);
  double z_ip=0.0, z_iq=0.0;          // elements of vector z
  double d_ip, d_iq;                  // elements of vector d = Lam
  double b_ip=0.0;                    // elements of vector b

  V.Identity();                       // Matrix V is initialized to the identity matrix
  double A_ii;                        // diagonal elements of A
  for (ip=0; ip<n; ip++)  {
    A_ii = A.getElement(ip,ip);
    b->setData(ip,A_ii);            // b and d are initialized
    d.setData(ip,A_ii);             // to diagonal elements of A
  }
  double A_pq;
  for (i=1; i<=50; i++)  {         //=============== main loop: max 50 Jacobi sweeps
    sm = 0.0;
    for (ip=0; ip<n-1; ip++)  {
      for (iq=ip+1; iq<n; iq++) {
        A_pq = A.getElement(ip,iq);
        sm += fabs(A_pq);     // sum of all off-diagonal elements of A (upper triangle)
      }
    }
    if (sm == 0)  {       // NORMAL return from jacobi:
      delete b;         // matrix is transformed into diagonal form
      delete z;
      return;
    }
    if (i < 4)
      tresh = 0.2 * sm / (n*n);
    else
      tresh = 0.0;
    for (ip=0; ip<n-1; ip++)  {
      for (iq=ip+1; iq<n; iq++)  {   // off-diagonal elements of matrix A
        A_pq = A.getElement(ip,iq);
        g = 100.0 * fabs (A_pq);
        d_ip = d.getData(ip);
        d_iq = d.getData(iq);
        if (i > 4 && (fabs(d_ip)+g) == fabs(d_ip)
            && (fabs(d_iq)+g) == fabs(d_iq))
          A.setElement(ip,iq, 0.0);
        else if (fabs(A.getElement(ip,iq)) > tresh)  {
          h = d.getData(iq) - d.getData(ip);
          if ((fabs(h)+g) == fabs(h))
            t = A.getElement(ip,iq) / h;
          else  {
            theta = 0.5 * h / A.getElement(ip,iq);
            t = 1.0 / (fabs(theta)+sqrt(1.0+theta*theta));
            if (theta < 0.0)
              t = -t;
          }
          c = 1.0 / sqrt(1+t*t);
          s = t * c;
          tau = s / (1.0 + c);
          h = t * A.getElement(ip,iq);
          z_ip = z->getData(ip) - h;
          z_iq = z->getData(iq) + h;
          d_ip = d.getData(ip) - h;
          d_iq = d.getData(iq) + h;
          z->setData(ip, z_ip);
          z->setData(iq, z_iq);
          d.setData(ip, d_ip);
          d.setData(iq, d_iq);
          A.setElement(ip,iq, 0.0);
          for (j=0; j<ip; j++)  {
            ROTATE(A,j,ip,j,iq)
          }
          for (j=ip+1; j<iq; j++)  {
            ROTATE(A,ip,j,j,iq)
          }
          for (j=iq+1; j<n; j++)  {
            ROTATE(A,ip,j,iq,j)
          }
          for (j=0; j<n; j++)  {
            ROTATE(V,j,ip,j,iq)
          }
          ++(nrot);
        } // end else if (Apq > tresh)
      } // end for (iq = ip+1 ...)
    }  // end if (ip = 0; ...)
    for (ip=0; ip<n; ip++)  {
      z_ip   = z->getData(ip);
      b_ip = b->getData(ip) + z_ip;
      b->setData(ip, b_ip);
      d.setData(ip, b_ip);
      z->setData(ip,0.0);
    }
  }    //------------------------- end of the main loop: for (i=1; i< 50; i++)
  //--- bad way out
  cerr << "\nToo many iterations in routine standard JACOBI" << endl;
}

//================ eigsrt
//------------------------------------------------------------------------------
/**
**  \brief eigsrt: sorting eigenvalues/eigenvectors in ascending order
**
**  INPUT:
**  Unsorted eigenvalues in Vector Lam(n) and
**  corresponding eigenvector matrix X(n,n)
**  (as obtained from JACOBI or TQLI)
**  OUTPUT:
**  Sorted into ascending order eigenvalues in Vector Lam(n) and
**  correspondingly sorted eigenvector matrix X(n,n)
**  Note: Vector Lam(n) ans Matrix X(n,n) are the private members of EigVal class
**/
//-------------------------------------------------------------------------------
void Jacobi::eigsrt_Jacobi()
{
  int k, j, i;
  double p;
  int n = d.getLen();

  for (i=0; i<n-1; i++)  {
    k = i;
    p = d.getData(k);               // p = d(k)
    for (j=i+1; j<n; j++) {
      if (d.getData(j) <= p) {    // d(j) <= p
        k = j;
        p = d.getData(k);       // p = d(k)
      }
    }   // end for j = i+1 ...
    if (k != i)  {
      d.setData(k,d.getData(i));   // d(k) = d(i)
      d.setData(i,p);                // d(i) = p
      for (j=0; j<n; j++)  {
        p = V.getElement(j,i);                  // p = V(j,i)
        V.setElement(j,i,V.getElement(j,k));    // V(j,i) = V(j,k)
        V.setElement(j,k,p);                    // V(j,k) = p
      }
    }  // end for k != i
  }   // end for i = 0 ...
}

//! generalized Jacobi transformation
void Jacobi::genJacobi()
{}

//
//===================== EXTERNAL EIGENVALUE ROUTINES
//
//=========== STANDARD EIGENVALUE PROBLEMS
//            (complete solution)
//----------- Standard Jacobi transformation
//
//--------------------------------------------------------------------------
/**
**  \brief Standard Jacobi transformation - EXTERNAL function
**
**   All eigenvalues and eigenvectors or a real symmetrix matrix
**   INPUT:
**   Matrix A(n,n) - real symmetrix matrix of order n
**   
**   OUTPUT:
**   Matrix V(n,n) - eigenmatrix - modal matrix (columns of X are eigenvectors of A)
**   Vector d(n)   - eigenvalues - vector containing eigenvalues of Matrix A
**   Notes: 
**   - elements of input Matrix A above the diagonal are destroyed
**   - int "nrot" contains the number of Jacobi rotations
**/
//--------------------------------------------------------------------------
void stJacobi (Matrix& A, Vector& d, Matrix& V, int *nrot)
{
  int j, iq, ip, i;
  int n = A.getRows();
  assert (n == A.getCols());       // input matrix must be square
  double tresh, theta, tau, t, sm, s, h, g, c;
  Vector* b = new Vector(n);
  Vector* z = new Vector(n);
  double z_ip=0.0, z_iq=0.0;       // elements of vector z
  double d_ip, d_iq;               // elements of vector d
  double b_ip=0.0;                 // elements of vector b

  V.setRows(n);
  V.setCols(n);
  V.Identity();                     // Matrix V is initialized to the identity matrix
  d.setSize(n);

  double A_ii;                       // diagonal elements of A
  for (ip=0; ip<n; ip++)  {
    A_ii = A.getElement(ip,ip);
    b->setData(ip,A_ii);            // b and d are initialized to diagonal elements of A
    d.setData(ip,A_ii);
  }
  *nrot = 0;
  double A_pq;                          // off-diagonal elements of A
  for (i=1; i<=50; i++)  {
    sm = 0.0;
    for (ip=0; ip<n-1; ip++)  {
      for (iq=ip+1; iq<n; iq++) {
        A_pq = A.getElement(ip,iq);
        sm += fabs(A_pq);          // sum off-diagonal elements of A
      }
    }
    if (sm == 0)  {
      delete b;
      delete z;
      return;
    }
    if (i < 4)
      tresh = 0.2 * sm / (n*n);
    else
      tresh = 0.0;
    for (ip=0; ip<n-1; ip++)  {
      for (iq=ip+1; iq<n; iq++)  {
        A_pq = A.getElement(ip,iq);
        g = 100.0 * fabs (A_pq);
        d_ip = d.getData(ip);
        d_iq = d.getData(iq);
        if (i > 4 && (fabs(d_ip)+g) == fabs(d_ip)
            && (fabs(d_iq)+g) == fabs(d_iq))
          A.setElement(ip,iq, 0.0);
        else if (fabs(A.getElement(ip,iq)) > tresh)  {
          h = d.getData(iq) - d.getData(ip);
          if ((fabs(h)+g) == fabs(h))
            t = A.getElement(ip,iq) / h;
          else  {
            theta = 0.5 * h / A.getElement(ip,iq);
            t = 1.0 / (fabs(theta)+sqrt(1.0+theta*theta));
            if (theta < 0.0)
              t = -t;
          }
          c = 1.0 / sqrt(1+t*t);
          s = t * c;
          tau = s / (1.0 + c);
          h = t * A.getElement(ip,iq);
          z_ip = z->getData(ip) - h;
          z_iq = z->getData(iq) + h;
          d_ip = d.getData(ip) - h;
          d_iq = d.getData(iq) + h;
          z->setData(ip,z_ip);
          z->setData(iq,z_iq);
          d.setData(ip,d_ip);
          d.setData(iq,d_iq);
          A.setElement(ip,iq, 0.0);
          for (j=0; j<ip; j++)  {
            ROTATE(A,j,ip,j,iq)
          }
          for (j=ip+1; j<iq; j++)  {
            ROTATE(A,ip,j,j,iq)
          }
          for (j=iq+1; j<n; j++)  {
            ROTATE(A,ip,j,iq,j)
          }
          for (j=0; j<n; j++)  {
            ROTATE(V,j,ip,j,iq)
          }
          ++(*nrot);
        }
      }
    }
    for (ip=0; ip<n; ip++)  {
      z_ip = z->getData(ip);
      b_ip = b->getData(ip) + z_ip;
      b->setData(ip, b_ip);
      d.setData(ip, b_ip);
      z->setData(ip, 0.0);
    }
  }
  cerr << "\nToo many iterations in routine standard JACOBI" << endl;
}

//--------- eigsrt
//------------------------------------------------------------------------------------
/**
**  \brief eigsrt: sorting eigenvalues/eigenvectors in ascending order - EXTERNAL function
**
**  INPUT:
**  Unsorted eigenvalues in Vector Lam(n) and
**  corresponding eigenvector matrix X(n,n)
**  (as obtained from JACOBI or TQLI)
**  OUTPUT:
**  Sorted into ascending order eigenvalues in Vector Lam(n) and
**  correspondingly sorted eigenvector matrix X(n,n)
**/
//----------------------------------------------------------------------------------
void eigsrt_Jacobi (Vector& d, Matrix& V)
{
  int k, j, i;
  double p;
  int n = d.getLen();

  for (i=0; i<n-1; i++)  {
    k = i;
    p = d.getData(k);               // p = d(k)
    for (j=i+1; j<n; j++) {
      if (d.getData(j) <= p) {    // d(j) <= p
        k = j;
        p = d.getData(k);       // p = d(k)
      }
    }
    if (k != i)  {
      d.setData(k,d.getData(i));   // d(k) = d(i)
      d.setData(i,p);             // d(i) = p
      for (j=0; j<n; j++)  {
        p = V.getElement(j,i);                 // p = V(j,i)
        V.setElement(j,i,V.getElement(j,k));    // V(j,i) = V(j,k)
        V.setElement(j,k,p);                   // V(j,k) = p
      }
    } // end for k != i
  }  // end for i = 0 ...
}

}   // end of namespace smv_lib
//============================= end fo file
